% Devin Koepl

classdef SlipModelClass < handle
    
    properties (Constant)
        g   = 9.81;
        
        ri  = 1;
        dri = 2;
        oi  = 3;
        doi = 4;
    end
    
    properties
        m;
        k;
        x0;
        
        sol;
    end
    
    methods
      
        function obj = SlipModelClass(m, k, r0, h, otd, vx)
           obj.m = m;
           obj.k = k;
           
           y = r0 * sin(otd);
           vy = - sqrt(2 * obj.g * (h - y));
           R = [cos(otd), sin(otd) ;
                 -sin(otd), cos(otd)   ];
           v = R * [vx; vy];
           vr = v(1);
           vo = v(2) / r0;
                      
           obj.x0 = [ r0, vr, otd, vo ];
           
           options = odeset('events', @obj.slip_model_events);
           obj.sol = ode45(@obj.slip_model_dynamics, [0 inf], obj.x0, options);
        end
        
        function x = state(obj, t)
           % Clamp off times before or after stance.
           t = max(0, min(obj.sol.xe, t));
            
           x = deval(obj.sol, t)';
        end
        
        function psiddot = angular_acceleration( obj, t )
           x = obj.state(t);    
           
           psiddot = ( -2 * x(:, obj.dri) .* x(:, obj.doi)...
               - obj.g * cos(x(:, obj.oi)) ) ./ x(:, obj.ri);
        end
        
        % Return [ r; dr/dt; d^2r/dt^2; d^3r/dt^3; d^4r/dt^4 ]
        function rd = radial_derivatives( obj, t )
            x = obj.state( t );
            oddot = obj.angular_acceleration( t );
            
            rd = zeros( 5, 1 );
            
            rd( 1 : 2 ) = x( [ obj.ri, obj.dri ] );
            rd(3) = ( obj.k / obj.m ) * ( obj.x0( obj.ri ) - x( obj.ri ) )...
                + x( obj.ri ) * x( obj.doi ) ^ 2 ...
                - obj.g * sin( x ( obj.oi ) );
            rd(4) = - obj.k / obj.m * x( obj.dri )...
                - obj.g * x( obj.doi ) * cos( x( obj.oi ) )...
                + x( obj.dri ) * x( obj.doi )^2 / obj.m...
                + 2 / obj.m * x( obj.ri ) * x( obj.doi ) * oddot;
            rd(5) = - obj.k / obj.m * rd(3)...
                - obj.g * oddot * cos( x( obj.oi ) )...
                - obj.g * x( obj.doi )^2 * sin( x( obj.oi ) )...
                - 2 * obj.g / obj.m * oddot * cos( x( obj.oi ) )...
                + 2 * obj.g * x( obj.doi )^2 * sin( x( obj.oi ) )...
                - 3 / obj.m * rd(3) * x( obj.doi )^2 ...
                - 6 / obj.m * x( obj.ri ) * x( obj.doi ) * oddot;
        end
        
        function sd = spring_deflection(obj, t)
           x = obj.state(t); 
           
           sd = [ obj.x0(obj.ri) - x(:, obj.ri); 
               x(:, obj.dri); 
               (obj.k/obj.m) * (obj.x0(obj.ri) - x(obj.ri))...
                    + x(obj.ri) * x(obj.doi)^2 ...
                    - obj.g * sin(x(obj.oi));                    ];
        end
        
        function [F, I, P] = force(obj, t)
           x = obj.state(t); 
            
           F = obj.k * ( obj.x0(obj.ri) - x(:, obj.ri) );  
           I = x(:, obj.dri) - obj.x0(obj.dri);
           P = x(:, obj.ri) - obj.x0(obj.ri);
        end
        
        function xdot = slip_model_dynamics(obj, t, x)  
            xdot = [x(obj.dri);
                (obj.k/obj.m) * (obj.x0(obj.ri) - x(obj.ri))...
                    + x(obj.ri) * x(obj.doi)^2 ...
                    - 9.81 * sin(x(obj.oi));
                x(obj.doi);
                ( -2 * x(obj.dri) * x(obj.doi)...
                    - obj.g * cos(x(obj.oi)) ) / x(obj.ri); ];    
        end

        function [value,isterminal,direction] = slip_model_events(obj, t, x)             
            value = [x(obj.oi) - pi; x(obj.oi); obj.x0(obj.ri) - x(obj.ri)];
            isterminal = [1; 1; 1];   
            direction = [1; -1; -1];   
        end    
        
    end
    
end